#pragma once

#include <iostream>

#include "../core/i_alloc.h"
#include "../numeric/i_lu.h"
#include "../ip/i_transform.h"
#include "../ip/i_filter.h"

//#ifdef _DEBUG
//#include <imdebug.h>
//#endif

#define I_DEFAULT_ALIGNMENT_MAX_ITER   100
#define I_DEFAULT_ALIGNMENT_EPSILON1   1e-3
#define I_DEFAULT_ALIGNMENT_EPSILON2   1e-1

namespace idl
{
  /*This class implements the "Inverse Compositional Image Alighment algorithm as described in
  "Equivalence and Efficiency of Image Alignment Algorithms, Simon Baker and Iain Matthews, CVPR 2001"*/
  template<typename MODELTYPE, int m/*# of unknows in the warpping function*/>
  class InvComposAlignmentCore
  {
  public:
    static const int _mm = m * m;
    explicit InvComposAlignmentCore(int n) : _n(n), _max_iter(I_DEFAULT_ALIGNMENT_MAX_ITER), _nr_iter(0), _status(0), _verbose(true)
    {
      if (!n)
      {
        _status = 1;
      }
      i_zero(_model, m);
      i_zero(_dmodel, m);
    }
    virtual ~InvComposAlignmentCore(){ cleanup(); };
    template <typename F>
    bool optimize(F& obj_func, const MODELTYPE model[m]/*initial guess of the model parameters*/);
    void getModel(MODELTYPE model[m]) const { if (model != NULL) i_copy(_model, model, m); }
    int getNumIter() const { return _nr_iter; }
    void setMaxFuncEv(int max_iter) { _max_iter = max_iter; }
    int  getMaxFuncEv() const { return _max_iter; }
    void setVerbose(bool verbose) { _verbose = verbose; }
    bool getVerbose() const { return _verbose; }
    int getStatus() const { return _status; }
  protected:
    void cleanup()
    {
      _n = 0;           /*number of residuals*/
      _max_iter = I_DEFAULT_ALIGNMENT_MAX_ITER;     /* maximum number of function evaluations*/
      _nr_iter = 0;     /* number of iterations*/
      _status = 0;      /* optimizer status*/
      _verbose = true;    /* if output intermediate results and status*/
    };
    void computeHessian(const MODELTYPE *J);
    void computeIncrements(const MODELTYPE *J, const MODELTYPE *Ie);
  protected:
    int _n;            /*number of residuals*/
    int _max_iter;     /* maximum number of function evaluations*/
    int _nr_iter;     /* number of iterations*/
    int _status;      /* optimizer status*/
    bool _verbose;    /* if output intermediate results and status*/
    /*internal memories:*/
    MODELTYPE  _model[m], _dmodel[m];
    MODELTYPE  _H[_mm], _Hi[_mm]; /*the mxm Hessian matrix and its mxm inverse*/
  };

  template<typename MODELTYPE, int m/*# of unknows in the warpping function*/>
  void InvComposAlignmentCore<MODELTYPE, m>::computeHessian(const MODELTYPE *J)
  {
    int i = 0;
    i_zero(_H, _mm);
    MODELTYPE  JJ[_mm];
    const MODELTYPE* ptrJ = J; /*J is a mxn matrix*/
    /*iterate every pixel*/
    for (; i < _n; ++i)
    {
      i_mult_AtA(ptrJ, JJ, 1, m);
      i_add(JJ, _H, _mm);
      ptrJ += m;
    }
    /*compute inverse of H*/
    switch (m)
    {
    case 1: _Hi[0] = i_rec(_H[0]);    break;
    case 2: i_lu_invert_2x2(_H, _Hi); break;
    case 3: i_lu_invert_3x3(_H, _Hi); break;
    case 4: i_lu_invert_4x4(_H, _Hi); break;
    case 5: i_lu_invert_5x5(_H, _Hi); break;
    case 6: 
    case 7: 
    case 8: 
    case 9: i_lu_invert<MODELTYPE, m>(_H, _Hi); break;
    default: _status = 2;
             std::cout << "Warning: currently this class only supports warpping model with up to 9 model parameters" << std::endl;
             break;
    }

    MODELTYPE II[_mm];
    i_mult_AB(_H, _Hi, II, m, m, m);

  }

  template<typename MODELTYPE, int m/*# of unknows in the warpping function*/>
  void InvComposAlignmentCore<MODELTYPE, m>::computeIncrements(const MODELTYPE *J, const MODELTYPE *Ie)
  {
    int i;
    i_zero(_dmodel, m);
    const MODELTYPE* ptrJ = J;
    const MODELTYPE* ptrE = Ie;
    MODELTYPE  Jg[m], HiJg[m];
    for (i = 0; i < _n; ++i)     /*iterate every pixel*/
    {
      if (ptrE[0] < Constant<MODELTYPE>::MAX_VAL()) /*invalid piexl has error equals to very large value*/
      {
        i_scale(ptrJ, Jg, m, ptrE[0]);
        i_mult_Ax(_Hi, Jg, HiJg, m, m);
        i_add(HiJg, _dmodel, m);
      }
      ptrJ += m;
      ptrE++;
    }
  }

  template<typename MODELTYPE, int m/*# of unknows in the warpping function*/>
  template <typename F>
  bool InvComposAlignmentCore<MODELTYPE, m>::optimize(F& obj_func, 
                                                                const MODELTYPE model[m]/*initial guess of the model parameters*/)                                                            
  {
    MODELTYPE model_tmp[m];
    MODELTYPE error_prev = 0, error = 0, error_delta = 0;
    bool terminate;
    i_copy(model, model_tmp, m);
    i_copy(model, _model,    m);
    
    /*set the initial delta model parameters to zeros*/
    /*Note that the formulation assumes the initial guess for dX is 0, we assume X0 is correct*/
    i_zero(_dmodel, m);

    /*compute the Jacobian dw/dp (evaluated at (x:0))*/
    obj_func.computeJacobian(model);
    computeHessian(obj_func.getJacobian()); /*compute Hessian only once*/

    if (_status != 0)
    {
      return false;
    }

    error_prev = obj_func.computeErrorImage(_model);

    if (_verbose)
    {
      std::cout << "Initial error (sum of squared difference SSD): " << error_prev << std::endl;
    }

    _nr_iter = 0;
    terminate = (error_prev < I_DEFAULT_ALIGNMENT_EPSILON2);
    
    while (!terminate)
    {
      /*compute model increments - _dmodels*/
      computeIncrements(obj_func.getJacobian(), obj_func.getErrorMap());
      
      /*update warpping model*/
      obj_func.updateModel(_dmodel, model_tmp);

      error = obj_func.computeErrorImage(model_tmp);

      if (_verbose)
      {
        std::cout << "Error: " << error << " at " << _nr_iter << " th iteration..." << std::endl;
      }

      if (error >= error_prev)
      {
        if (_verbose)
        {
          std::cout << "Error converged at: " << error_prev << " in " << _nr_iter << " iterations" << std::endl;
        }
        break;
      }
      else
      {
        i_copy(model_tmp, _model, m);
        error_delta = (error_prev - error) / error_prev; /*must be positive since error_prev > error*/
        error_prev = error;
      }
      ++_nr_iter;
      terminate = (_nr_iter >= _max_iter) || (error_delta < I_DEFAULT_ALIGNMENT_EPSILON1) || (error_prev < I_DEFAULT_ALIGNMENT_EPSILON2);
    }
    return true;
  }

  /*Alignment warpping model for the Euclidean (3-DOF) transformation
    [R, t
     0, 1]*/
  template<typename MODELTYPE>
  class InvComposAlignmentEuclidean
  {
  public:
    static const int _m = 3; 
    InvComposAlignmentEuclidean(int width, int height) : _width(width), _height(height), _cx(width / 2), _cy(height / 2), _initialized(false), _solver(NULL), _Iw(NULL), _I(NULL), _TPLT(NULL), _Gx(NULL), _Gy(NULL)
    {
      int x, y, dx, dy;
      const int n = _width * _height;
      _Iw = i_alloc2<unsigned char>(_height, _width);
      
      _solver = new InvComposAlignmentCore<MODELTYPE, _m>(n);
      _J = i_alloc<MODELTYPE>(n*(_m + 1));
      _Ie = _J + (n*_m);
      
      _Gx = i_alloc<int>((n << 1));
      _Gy = _Gx + n;

      if (_Iw != NULL && _J != NULL && _solver != NULL && _solver->getStatus() == 0)
      {
        _initialized = true;
      }
      else
      {
        cleanup();
      }
      /*compute the pixel pre-conditioning parameters*/
      MODELTYPE avg_distance = (MODELTYPE)0.0;
      for (y = 0; y < _height; ++y)
      {
        dy = i_sqr(y - _cy);
        for (x = 0; x < _width; ++x)
        {
          dx = i_sqr(x - _cx);
          avg_distance += i_sqrt(MODELTYPE(dx + dy));
        }
      }
      avg_distance = avg_distance / n;
      MODELTYPE sf = Constant<MODELTYPE>::SQRT_2() * i_rec(avg_distance);
      /*construct the image coordinate transformation (preconditioning) matrix*/
      i_upper_triangular_3x3(sf, (MODELTYPE)0.0, (-sf*_cx), sf, (-sf*_cy), (MODELTYPE)1.0, _T);
      i_invert_3x3_upper_triangular(_T, _Ti);
    }

    virtual ~InvComposAlignmentEuclidean(){ cleanup(); };
    bool setTemplate(const unsigned char *const *T);
    bool align(const unsigned char *const *I, const MODELTYPE model[_m], MODELTYPE H[9]);
   
    MODELTYPE computeErrorImage(const MODELTYPE model[_m]);
    bool computeJacobian(const MODELTYPE model[_m]);
    void updateModel(const MODELTYPE dmodel[_m], MODELTYPE model[_m]);
    const MODELTYPE* getJacobian() const { return _J; };
    const MODELTYPE* getErrorMap() const { return _Ie; }
    void setVerbose(bool verbose) { if (_initialized) _solver->setVerbose(verbose); }
    void setMaxFuncEv(int nr_iter) { if (_initialized) _solver->setMaxFuncEv(nr_iter); }
  
  protected:
    inline bool warp(const MODELTYPE model[_m]);
    void cleanup()
    {
      if (_solver){ delete _solver; _solver = NULL; }
      i_free<int>(_Gx);
      i_free2<unsigned char>(_Iw);
      
      i_free<MODELTYPE>(_J);
      _I = _TPLT = NULL;
      _Iw = NULL;
      _J = _Ie = NULL;
      _Gy = NULL;
      _width = _height = _cx = _cy = 0;
      _initialized = false;
    }
    inline void getWarpFunc(const MODELTYPE model[_m], MODELTYPE H[9])/*from the model parameters to the warpping function in 3x3 matrix form*/
    {
      MODELTYPE c = i_cos(model[0]);
      MODELTYPE s = i_sin(model[0]);
      MODELTYPE Hp[9];

      H[0] = c; H[1] = -s; H[2] = model[1];
      H[3] = s; H[4] = c; H[5] = model[2];
      H[6] = 0; H[7] = 0; H[8] = (MODELTYPE)1.0;

      /*denormalization*/
      i_mult_AB_3x3_3x3_w_B_upper_triangular(H, _T, Hp);
      i_mult_AB_3x3_3x3_w_A_upper_triangular(_Ti, Hp, H);
    }

  protected:
    int _width, _height, _cx, _cy;
    MODELTYPE _T[9], _Ti[9];/*the 2D preconditioning transformation matrix*/
    bool _initialized;
    const unsigned char **_I, **_TPLT;/*allocated from outside and assigned to this object*/
    unsigned char **_Iw; /*working array to store the warpped image*/
    MODELTYPE *_Ie, *_J;
    int *_Gx, *_Gy;
    MODELTYPE _model[_m];
    InvComposAlignmentCore<MODELTYPE, _m> *_solver;
  };

  template<typename MODELTYPE>
  bool InvComposAlignmentEuclidean<MODELTYPE>::align(const unsigned char *const *I, const MODELTYPE model[_m], MODELTYPE H[9])
  {
    if (!_initialized || !I || !model || !_TPLT/*check if template image has been set*/)
    {
      return false;
    }
    _I = (const unsigned char **)I;
    bool ret = _solver->optimize(*this, model);
    if (ret)
    {
      _solver->getModel(_model);/*model in the normalized image space*/
      getWarpFunc(_model, H);
    }
    return ret;
  }

  template<typename MODELTYPE>
  bool InvComposAlignmentEuclidean<MODELTYPE>::setTemplate(const unsigned char *const *T)
  {
    if (!_initialized || !T)
    {
      return false;
    }
    _TPLT = (const unsigned char **)T;
    return true;
  }

  template<typename MODELTYPE>
  inline bool InvComposAlignmentEuclidean<MODELTYPE>::warp(const MODELTYPE model[_m])
  {
    MODELTYPE H[9], Hi[9];
    MODELTYPE c = i_cos(model[0]);
    MODELTYPE s = i_sin(model[0]);
    //MODELTYPE tx = model[1];
    //MODELTYPE ty = model[2];
    H[0] = c;  H[1] = -s;  H[2] = model[1];
    H[3] = s;  H[4] = c;   H[5] = model[2];
    H[6] = 0;  H[7] = 0;   H[8] = (MODELTYPE)1.0;

    if (i_equal_identity_3x3(H))
    {
      i_copy(_I[0], _Iw[0], _width*_height);
      return true;
    }

    /*denormalization*/
    i_mult_AB_3x3_3x3_w_B_upper_triangular(H,   _T, Hi);
    i_mult_AB_3x3_3x3_w_A_upper_triangular(_Ti, Hi,  H);
    
    i_lu_invert_affine_3x3(H, Hi);
    i_warp_lum_affine_bilinear(_I, Hi, _Iw, _width, _height, 0);
    return true;
  }

  template<typename MODELTYPE>
  bool InvComposAlignmentEuclidean<MODELTYPE>::computeJacobian(const MODELTYPE model[_m])
  {
    int x, y;
    MODELTYPE fx, fy, gx, gy, theta = model[0];
    MODELTYPE sf = _T[0], tx = _T[2], ty = _T[5], rec_sf = i_rec(sf);
    MODELTYPE sin_t = i_sin(theta);
    MODELTYPE cos_t = i_cos(theta);
    MODELTYPE cty, sty;
    MODELTYPE *pj = _J;
     
    /*compute gradient in the x and y directions*/
    i_centered_dx(_TPLT[0], _Gx, _width, _height);
    i_centered_dy(_TPLT[0], _Gy, _width, _height);

    const int* pdx = _Gx;
    const int* pdy = _Gy;

    for (y = 0; y < _height; ++y)
    {
      fy = sf*y + ty; 
      cty =-cos_t * fy;
      sty = sin_t * fy;
      for (x = 0; x < _width; ++x)
      {
        fx = sf*x + tx;

        /*note in the original image space, dx(dy) = 2.0; 
          but in the normalized image coordinates, the dx(dy) = 2 * sf, 
          dx(dy) = 2.0 has been taken care of in the i_centered_dx(dy) functions, 
          the additional normalization is done here by multiplying gx(gy) with 1/sf*/

        gx = rec_sf * (MODELTYPE)(*pdx++); 
        gy = rec_sf * (MODELTYPE)(*pdy++);

        /*compute Jacobian*/
        *pj++ = gx * (-sin_t*fx + cty) + gy * (cos_t*fx - sty);
        *pj++ = gx;
        *pj++ = gy;
      }
    }
    return true;
  }

  template<typename MODELTYPE>
  MODELTYPE InvComposAlignmentEuclidean<MODELTYPE>::computeErrorImage(const MODELTYPE model[_m])
  {
    MODELTYPE e, err = 0;
    const int n = _width * _height;
    int i = 0, count = n;

    if (!warp(model))
    {
      return Constant<MODELTYPE>::MAX_VAL();
    }
    
//#ifdef _DEBUG
//    else
//    {
//      imdebug("lum *auto b=8 w=%d h=%d %p", _width, _height, _Iw[0]);
//    }
//#endif
    const unsigned char *pw = _Iw[0];
    const unsigned char *pt = _TPLT[0];

    for (; i < n; ++i)
    {
      if (_Iw[0][i] > 0)/*empty pixel are given 0 value - ignored in counting total error*/
      {
        e = _Ie[i] = (MODELTYPE)(pw[i]) - (MODELTYPE)(pt[i]);
        err += e*e;
      }
      else
      {
        _Ie[i] = Constant<MODELTYPE>::MAX_VAL();
        --count;
      }
    }
    return count > 0 ? (err / count) : Constant<MODELTYPE>::MAX_VAL();
  }

  /*given the incremental warp update, update global warp model inplace*/
  template<typename MODELTYPE>
  void InvComposAlignmentEuclidean<MODELTYPE>::updateModel(const MODELTYPE dmodel[_m], MODELTYPE model[_m])
  {
    MODELTYPE W[9], dW[9], dWi[9], c, s, ctheta, stheta;
    c = i_cos(model[0]);
    s = i_sin(model[0]);
    
    W[0] = c; W[1] = -s; W[2] = model[1];
    W[3] = s; W[4] = c;  W[5] = model[2];
    W[6] = 0; W[7] = 0;  W[8] = (MODELTYPE)1.0;

    c = i_cos(dmodel[0]);
    s = i_sin(dmodel[0]);
    
    dW[0] = c; dW[1] = -s; dW[2] = dmodel[1];
    dW[3] = s; dW[4] = c;  dW[5] = dmodel[2];
    dW[6] = 0; dW[7] = 0;  dW[8] = (MODELTYPE)1.0;

    i_lu_invert_affine_3x3(dW, dWi);
    i_mult_AB_3x3_3x3(W, dWi, dW/*reuse this memory to store the new motion model*/);

    c = (i_trace_3x3(dW) - (MODELTYPE)1.0) / 2;
    ctheta = i_acos(c); /*range [0, PI]*/

    s = (-dW[1] + dW[3]) / 2;
    stheta = i_asin(s); /*range [-PI/2, PI/2]*/

    if (stheta < 0)
    {
      ctheta = -ctheta;
    }
    model[0] = ctheta;
    model[1] = dW[2];
    model[2] = dW[5];
  }

  /*-----------------------------------------------------------------*/
  /*Alignment warpping model for the Similarity (4-DOF) transformation
  [sR, t
    0, 1]
    Note that in this implementation the scale factor s is bounded in [0.0, 2] by assuming the scale factor should not be too small or too large.
    s is parameterized as s = 1+sin(model[0]), model[0] is a real number in [-PI/2, PI/2];
    */
  template<typename MODELTYPE>
  class InvComposAlignmentSimilarity
  {
  public:
    static const int _m = 4;
    InvComposAlignmentSimilarity(int width, int height) : _width(width), _height(height), _cx(width / 2), _cy(height / 2), _initialized(false), _solver(NULL), _Iw(NULL), _I(NULL), _TPLT(NULL), _Gx(NULL), _Gy(NULL)
    {
      int x, y, dx, dy;
      const int n = _width * _height;
      _Iw = i_alloc2<unsigned char>(_height, _width);

      _solver = new InvComposAlignmentCore<MODELTYPE, _m>(n);
      _J = i_alloc<MODELTYPE>(n*(_m + 1));
      _Ie = _J + (n*_m);

      _Gx = i_alloc<int>((n << 1));
      _Gy = _Gx + n;

      if (_Iw != NULL && _J != NULL && _solver != NULL && _solver->getStatus() == 0)
      {
        _initialized = true;
      }
      else
      {
        cleanup();
      }

      /*compute the pixel pre-conditioning parameters*/
      MODELTYPE avg_distance = (MODELTYPE)0.0;
      for (y = 0; y < _height; ++y)
      {
        dy = i_sqr(y - _cy);
        for (x = 0; x < _width; ++x)
        {
          dx = i_sqr(x - _cx);
          avg_distance += i_sqrt(MODELTYPE(dx + dy));
        }
      }
      avg_distance = avg_distance / n;
      MODELTYPE sf = Constant<MODELTYPE>::SQRT_2() * i_rec(avg_distance);
      /*construct the image coordinate transformation (preconditioning) matrix*/
      i_upper_triangular_3x3(sf, (MODELTYPE)0.0, -(sf*_cx), sf, -(sf*_cy), (MODELTYPE)1.0, _T);
      //i_invert_3x3_upper_triangular(_T, _Ti);
      i_lu_invert_3x3(_T, _Ti);
    }

    virtual ~InvComposAlignmentSimilarity(){ cleanup(); };
    bool setTemplate(const unsigned char *const *T);
    bool align(const unsigned char *const *I, const MODELTYPE model[_m]/*initial guess*/, MODELTYPE H[9]/*final 3x3 matrix form warpping function*/);
    MODELTYPE computeErrorImage(const MODELTYPE model[_m]);
    bool computeJacobian(const MODELTYPE model[_m]);
    void updateModel(const MODELTYPE dmodel[_m], MODELTYPE model[_m]);
    const MODELTYPE* getJacobian() const { return _J; };
    const MODELTYPE* getErrorMap() const { return _Ie; }
    void setVerbose(bool verbose) { if (_initialized) _solver->setVerbose(verbose); }
    void setMaxFuncEv(int nr_iter) { if (_initialized) _solver->setMaxFuncEv(nr_iter); }

  protected:
    inline bool warp(const MODELTYPE model[_m]);
    void cleanup()
    {
      if (_solver){ delete _solver; _solver = NULL; }
      i_free<int>(_Gx);
      i_free2<unsigned char>(_Iw);
      i_free<MODELTYPE>(_J);
      _I = _TPLT = NULL;
      _Iw = NULL;
      _J = _Ie = NULL;
      _Gy = NULL;
      _width = _height = _cx = _cy = 0;
      _initialized = false;
    }
    inline void getWarpFunc(const MODELTYPE model[_m], MODELTYPE H[9])/*from the model parameters to the warpping function in 3x3 matrix form*/
    {
      MODELTYPE a = (MODELTYPE)1.0 + i_sin(model[0]);
      MODELTYPE c = i_cos(model[1]);
      MODELTYPE s = i_sin(model[1]);
      MODELTYPE ac = a*c;
      MODELTYPE as = a*s;
      MODELTYPE Hp[9];

      H[0] = ac; H[1] = -as; H[2] = model[2];
      H[3] = as; H[4] = ac;  H[5] = model[3];
      H[6] = 0;  H[7] = 0;   H[8] = (MODELTYPE)1.0;

      /*denormalization*/
      i_mult_AB_3x3_3x3_w_B_upper_triangular(H, _T, Hp);
      i_mult_AB_3x3_3x3_w_A_upper_triangular(_Ti, Hp, H);
    }
  protected:
    int _width, _height, _cx, _cy;
    MODELTYPE _T[9], _Ti[9]; /*the 2D preconditioning transformation matrix*/
    bool _initialized;
    const unsigned char **_I, **_TPLT;/*allocated from outside and assigned to this object*/
    unsigned char **_Iw; /*working array to store the warpped image*/
    MODELTYPE *_Ie, *_J;
    int *_Gx, *_Gy;
    MODELTYPE _model[_m];
    InvComposAlignmentCore<MODELTYPE, _m> *_solver;
  };

  template<typename MODELTYPE>
  bool InvComposAlignmentSimilarity<MODELTYPE>::align(const unsigned char *const *I, const MODELTYPE model[_m], MODELTYPE H[9])
  {
    if (!_initialized || !I || !model || !_TPLT/*check if template image has been set*/)
    {
      return false;
    }
    _I = (const unsigned char **)I;
    bool ret = _solver->optimize(*this, model);
    if (ret)
    {
      _solver->getModel(_model);/*model in the normalized image space*/
      getWarpFunc(_model, H);
    }
    return ret;
  }

  template<typename MODELTYPE>
  bool InvComposAlignmentSimilarity<MODELTYPE>::setTemplate(const unsigned char *const *T)
  {
    if (!_initialized || !T)
    {
      return false;
    }
    _TPLT = (const unsigned char **)T;
    return true;
  }

  template<typename MODELTYPE>
  inline bool InvComposAlignmentSimilarity<MODELTYPE>::warp(const MODELTYPE model[_m])
  {
    MODELTYPE H[9], Hi[9];
    MODELTYPE a = (MODELTYPE)1.0 + i_sin(model[0]);
    MODELTYPE c = i_cos(model[1]);
    MODELTYPE s = i_sin(model[1]);
    MODELTYPE ac = a*c;
    MODELTYPE as = a*s;

    H[0] = ac; H[1] = -as; H[2] = model[2];
    H[3] = as; H[4] = ac;  H[5] = model[3];
    H[6] = 0;  H[7] = 0;   H[8] = (MODELTYPE)1.0;

    if (i_equal_identity_3x3(H))
    {
      i_copy(_I[0], _Iw[0], _width*_height);
      return true;
    }

    /*denormalization*/
    i_mult_AB_3x3_3x3_w_B_upper_triangular(H, _T, Hi);
    i_mult_AB_3x3_3x3_w_A_upper_triangular(_Ti, Hi, H);

    i_lu_invert_affine_3x3(H, Hi);
    i_warp_lum_affine_bilinear(_I, Hi, _Iw, _width, _height, 0);
    return true;
  }

  template<typename MODELTYPE>
  bool InvComposAlignmentSimilarity<MODELTYPE>::computeJacobian(const MODELTYPE model[_m])
  {
    int x, y;
    MODELTYPE fx, fy, gx, gy, a = (MODELTYPE)1.0 + i_sin(model[0]), theta = model[1];
    MODELTYPE sf = _T[0], tx = _T[2], ty = _T[5], rec_sf = i_rec(sf);
    MODELTYPE sin_t = i_sin(theta);
    MODELTYPE cos_t = i_cos(theta);
    MODELTYPE da = i_cos(model[0]);
    MODELTYPE asin_t = a * sin_t;
    MODELTYPE acos_t = a * cos_t;
    MODELTYPE cty, sty, acty, asty;
    MODELTYPE *pj = _J;

    /*compute gradient in the x and y directions*/
    i_centered_dx(_TPLT[0], _Gx, _width, _height);
    i_centered_dy(_TPLT[0], _Gy, _width, _height);

    const int* pdx = _Gx;
    const int* pdy = _Gy;

    for (y = 0; y < _height; ++y)
    {
      fy = sf*y + ty;

      cty = cos_t * fy;
      sty = sin_t * fy;

      acty = a * cty;
      asty = a * sty;

      for (x = 0; x < _width; ++x)
      {
        fx = sf*x + tx;

        /*note in the original image space, dx(dy) = 2.0;
        but in the normalized image coordinates, the dx(dy) = 2 * sf,
        dx(dy) = 2.0 has been taken care of in the i_centered_dx(dy) functions,
        the additional normalization is done here by multiplying gx(gy) with 1/sf*/

        gx = rec_sf * (MODELTYPE)(*pdx++);
        gy = rec_sf * (MODELTYPE)(*pdy++);

        /*compute Jacobian*/
        *pj++ = (gx * (cos_t*fx - sty) + gy * (sin_t*fx + cty))*da;
        *pj++ = gx * (-asin_t*fx - acty) + gy * (acos_t*fx - asty);
        *pj++ = gx;
        *pj++ = gy;

      }
    }
    return true;
  }

  template<typename MODELTYPE>
  MODELTYPE InvComposAlignmentSimilarity<MODELTYPE>::computeErrorImage(const MODELTYPE model[_m])
  {
    MODELTYPE e, err = 0;
    const int n = _width * _height;
    int i = 0, count = n;

    if (!warp(model))
    {
      return Constant<MODELTYPE>::MAX_VAL();
    }

//#ifdef _DEBUG
//    else
//    {
//      imdebug("lum *auto b=8 w=%d h=%d %p", _width, _height, _Iw[0]);
//    }
//#endif
    const unsigned char *pw = _Iw[0];
    const unsigned char *pt = _TPLT[0];

    for (; i < n; ++i)
    {
      if (_Iw[0][i] > 0)/*empty pixel are given 0 value - ignored in counting total error*/
      {
        e = _Ie[i] = (MODELTYPE)(pw[i]) - (MODELTYPE)(pt[i]);
        err += e*e;
      }
      else
      {
        _Ie[i] = Constant<MODELTYPE>::MAX_VAL();
        --count;
      }
    }
    return count > 0 ? (err / count) : Constant<MODELTYPE>::MAX_VAL();
  }

  /*given the incremental warp update, update global warp model inplace*/
  template<typename MODELTYPE>
  void InvComposAlignmentSimilarity<MODELTYPE>::updateModel(const MODELTYPE dmodel[_m], MODELTYPE model[_m])
  {
    MODELTYPE W[9], dW[9], dWi[9], c, s, a, ac, as, ctheta, stheta;
    a = (MODELTYPE)1.0 + i_sin(model[0]);
    c = i_cos(model[1]);
    s = i_sin(model[1]);
    ac = a*c;
    as = a*s;

    W[0] = ac; W[1] = -as; W[2] = model[2];
    W[3] = as; W[4] =  ac; W[5] = model[3];
    W[6] = 0;  W[7] =   0; W[8] = (MODELTYPE)1.0;

    a = (MODELTYPE)1.0 + i_sin(dmodel[0]);
    c = i_cos(dmodel[1]);
    s = i_sin(dmodel[1]);
    ac = a*c;
    as = a*s;

    dW[0] = ac; dW[1] = -as; dW[2] = dmodel[2];
    dW[3] = as; dW[4] =  ac; dW[5] = dmodel[3];
    dW[6] = 0;  dW[7] =   0; dW[8] = (MODELTYPE)1.0;

    i_lu_invert_affine_3x3(dW, dWi);
    i_mult_AB_3x3_3x3(W, dWi, dW/*reuse this memory to store the new motion model*/);

    ac = (i_trace_3x3(dW) - (MODELTYPE)1.0) / 2;
    as = (-dW[1] + dW[3]) / 2;

    a = i_sqrt((ac*ac) + (as*as)); //guaranteed positive scale factor
    c = i_div(ac, a);
    s = i_div(as, a);

    ctheta = i_acos(c); /*range [0, PI]*/
    stheta = i_asin(s); /*range [-PI/2, PI/2]*/

    if (stheta < 0)
    {
      ctheta = -ctheta;
    }

    model[0] = i_asin(a - (MODELTYPE)1.0);
    model[1] = ctheta;
    model[2] = dW[2];
    model[3] = dW[5];
  }

} /* namespace idl */